#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 4
#define SENSORPIN A5
#define SPKRPIN 3
#define RX_1 5
#define TX_1 6
#define RX_2 8
#define TX_2 9
#define RX_3 10
#define TX_3 11
#define RX_4 12
#define TX_4 13
#define MAX_BUFFER_SIZE 64
#define MSG_RCVD 102
#define LISTENING 0
#define CHANNEL_SET 1


SoftwareSerial mySerial[4] = {SoftwareSerial(RX_1,TX_1),SoftwareSerial(RX_2,TX_2),SoftwareSerial(RX_3,TX_3),SoftwareSerial(RX_4,TX_4)};

int active_channel;
int listen_count;
int state;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len, write_buf_len;
long waitTime;
// int isFirst;
// long rand100;
// int collecting_IDs;
// uint8_t IDs[17];
// uint8_t ID_read[4];
uint8_t myID;
// int id_count;
// int noNewID = 0;
// char msg_buf[16] = "TEST_MESSAGE";
long prevTime;
long startTime;
// long time_1;
// long time_2;
int ledState = LOW;
// uint8_t buf_first;

void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(byte val, byte *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return 1;
    }
    return 0;
}

//Write message buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  int temp_len = buf_size;
  uint8_t temp_buf[buf_size+1];
  const uint8_t start_sig = 170;
  while(temp_len > 0){
    temp_buf[temp_len] = arr[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(i=0;i<buf_size+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  if(channel > 0 && channel < 5){
    mySerial[channel-1].write(temp_buf, buf_size+1);
  }else{
    Serial.println("Invalid channel selection for writebuffertoserial()");
    Serial.print("Error! cannot write to serial channel ");
    Serial.println(channel);
  }
  /* switch(channel){
    case 1:
      mySerial_1.write(temp_buf, buf_size+1);
      break;
    case 2:
      mySerial_2.write(temp_buf, buf_size+1);
      break;
    case 3:
      mySerial_3.write(temp_buf, buf_size+1);
      break;
    case 4:
      mySerial_4.write(temp_buf, buf_size+1);
      break;
    default:
      Serial.println("Invalid channel selection for writebuffertoserial()");
      Serial.print("Error! cannot write to serial channel ");
      Serial.println(channel);
      break;
    } */
}

int getChannel(){
    
    return active_channel;
}
int setChannel(int channel){
    if(channel > 0 && channel < 5){
        mySerial[channel-1].listen();
        delay(100);
        active_channel = channel;
        return 0;
    }else{
        return -1;
    }
}

void send_msg_received(){
    uint8_t msg_rcvd = MSG_RCVD;
    for(int i=0;i<2;i++){
        writebuffertoserial(active_channel, &msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption the our confirmation was received
}

/* void checkID(uint8_t ID){
  if(ID > 0 && ID <= 100){
    if(ID > IDs[0] || isFirst != 1){
      digitalWrite(LEDPIN, LOW);
      isFirst = 0;
    }else{
      digitalWrite(LEDPIN, HIGH);
    }
    if(!isvalueinarray(ID, IDs, id_count)){
      IDs[id_count] = ID;
      id_count++;
      Serial.print(ID, DEC);
      Serial.println(" added"); // to the ID list.");
      Serial.print("id_count "); //now equals: ");
      Serial.println(id_count, DEC);
      noNewID = 0;
    }else{
      noNewID++;
      if(noNewID > 50){
        Serial.print(id_count, DEC);
        Serial.println(" IDs found");
        collecting_IDs = 0;
      }
    }
  }
} */

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT);
  pinMode(TX_4, OUTPUT);
  for(int i=3;i>=0;i--){
    mySerial[i].begin(TRXRATE);
    Serial.println("...");
  }
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  Serial.println("Generating random ID...");
  //randomSeed(analogRead(A0));
  //rand100 = random(1,101);
  //IDs[0] = uint8_t(rand100);
  //IDs[1] = uint8_t(76);
  //id_count = 1;
  //collecting_IDs = 1;
  //isFirst = 1;
  //msg_buf[12] = 0;
  myID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  prevTime = millis();
  waitTime = 20;
  read_buf_len = write_buf_len = 0;
  state = LISTENING;
}

void loop(){
  
  switch(state){
    case LISTENING:
        //in listening mode, cycle through all ports until info is received
        setChannel(listen_count);
        Serial.println(active_channel,DEC);
        startTime = millis();
        while(mySerial[active_channel-1].available() == 0 && ((millis() - startTime) < waitTime)){}
        if(mySerial[active_channel-1].available() > 0){
            state = CHANNEL_SET;
            Serial.println("mode: CHANNEL_SET");
        }else{
            listen_count++;
            delay(3);
        }
        if(listen_count > 4){
            listen_count = 1;
        }
        break;
    case CHANNEL_SET:
        Serial.println(active_channel);
        Serial.println(mySerial[active_channel-1].peek(),DEC);
        if(mySerial[active_channel-1].read() == 170){
            int cnt=0;
            while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE){
                read_buf[cnt] = mySerial[active_channel-1].read();
                cnt++;
            }
            read_buf_len = cnt;
            send_msg_received();
            state = LISTENING;
            Serial.println("mode: LISTENING");
        }else{
            //mySerial[active_channel-1].flush();
            while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
        }
        break;
    default:
        break;
  }
  
/*   if(collecting_IDs){
    //mySerial_1.write(msg_buf, 12);
    mySerial_1.listen();
    delay(1);
    writebuffertoserial(1, IDs, id_count);
    //writebuffertoserial(2, IDs, id_count);
    delay(7);
    Serial.print("The next value in the buffer for channel 1 is ");
    Serial.println(mySerial_1.peek());
    if(mySerial_1.read() == 170){
      while(mySerial_1.available() > 0){
        ID_read[0] = mySerial_1.read();
        //Serial.print(ID_read[0]);
        checkID(ID_read[0]);
      }
      //Serial.println("***");
    }
    mySerial_2.listen();
    delay(1);
    //writebuffertoserial(1, IDs, id_count);
    writebuffertoserial(2, IDs, id_count);
    delay(7);
    Serial.print("The next value in the buffer for channel 2 is ");
    Serial.println(mySerial_2.peek(), DEC);
    if(mySerial_2.read() == 170){
      while(mySerial_2.available() > 0){
        ID_read[1] = mySerial_2.read();
        checkID(ID_read[1]);
      }
      //Serial.println("***");
    }
  }
  if(isFirst == 1){
    blink_led(LEDPIN);
  }else{
    digitalWrite(LEDPIN, LOW);
  }
  //writebuffertoserial(1, IDs, id_count);
  //delay(7);
  //writebuffertoserial(2, IDs, id_count);
  delay(1); */
}
